#pragma once
#include "rclcpp/rclcpp.hpp"
// #include "interfaces/srv/armor.hpp"
// #include "interfaces/msg/imgpose.hpp"
#include "interfaces/msg/armor.hpp"
#include "interfaces/msg/readpack.hpp"
#include "interfaces/msg/pose.hpp"
#include "interfaces/msg/sendpack.hpp"
// #include "interfaces/msg/clocknow.hpp"
#include "interfaces/msg/autoaim_recorder.hpp"
#include "interfaces/msg/tovision.hpp"
#include "target_solve/targetsolver.h"
#include "antigyro/anti_gyro.h"
#include "../../tool/include/util_func.h"
#include "armor_predict/armor_predict.h"
#include "dynamic_hit_outpost/dynamic_hit_outpost.h"
#include "aimbot/aimbot.h"
#include "armor/armor.h"
#include "autoaim/tools.h"
#include <map>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.hpp>
#include <filesystem>

#include "geometry_msgs/msg/pose_stamped.hpp"

#define IF_CALIB 0
#define SAMPLE_SIZE 10


class AutoAim : public rclcpp::Node
{
public:
    std::string workspace_path;
    double clock_now_new;
    // 声明节点
    CircleQueue<Target, 4> target_queue;
    AntiGyro anti_gyror;
    double delta_time;
    Target last_target;
    Pose pose;
    SendPack send_pack;
    ReadPack work_msg;
    std::vector<Armor> armors;
    AimBot armor_detector;
    ArmorPredictor armor_predictor;
    TargetSolver target_solver;
    ArmorDir dir;
    DynamicHitOutpost dynamic_hit_outposter;
    // 击打前哨站类
    // Outpost_func outpostFunc;
    double time_process;
    int mode = 1;
    int fps;
    int pair_time_cnt;
    int cnt;
    int car_id;
    int synchronizor = 0;
    int frame_width = 0, frame_height = 0;
    double tmp_xy_dis = 0;
    double tmp_bullet_speed = 22;
    double last_bullet_speed = 22;
    int armor_num = 4;
    double start_time = 0;
    double time_1 = 0;
    double time_2 = 0;
    double end_time = 0;

    interfaces::msg::AutoaimRecorder autoaim_recorder;

    // std::map<double, interfaces::msg::Imgpose::SharedPtr> imgpose_dictionary;
    // std::map<double, interfaces::msg::Readpack::SharedPtr> readpack_dictionary;
    CircleQueue<interfaces::msg::Readpack, 101> queue_readpack; // 电控姿态包队列，注意里面最多只有100个有效值！

    CircleQueue<bool, 101> queue_armor_found; // 哨兵计算识别率用

    // circularQueue<double> queue_ekf_omega;//计算平均角速度用

    unsigned char control_mode = false;
    std::vector<unsigned char> queue_hit_priority;
    std::vector<unsigned char> queue_dishit; // 哨兵双机通信用

    // rclcpp::Service<interfaces::srv::Armor>::SharedPtr autoaim_server_;
    rclcpp::Subscription<interfaces::msg::Armor>::SharedPtr sub_autoaim;
    rclcpp::Subscription<interfaces::msg::Readpack>::SharedPtr readpack_subscriber;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_debugimage;
    rclcpp::Subscription<interfaces::msg::Tovision>::SharedPtr sub_tovision;
    // rclcpp::Subscription<interfaces::msg::Clocknow>::SharedPtr subscriber_time;
    rclcpp::Publisher<interfaces::msg::Sendpack>::SharedPtr publisher_Sendpack;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_img;
    rclcpp::Publisher<interfaces::msg::AutoaimRecorder>::SharedPtr pub_recorder;

    // 烧饼信息融合测试
    rclcpp::Publisher<interfaces::msg::Tobase>::SharedPtr pub_tobase;
    rclcpp::TimerBase::SharedPtr timer_;
    interfaces::msg::Tobase tb;
    
    // functions
    //  构造函数,有一个参数为节点名称
    AutoAim(std::string name);
    
    void init_global_param();

    void time_pair();

    void calculate_bullet_speed(double &bullet_speed);

    void linear_predict_pose(interfaces::msg::Pose &result, interfaces::msg::Pose &pose1, interfaces::msg::Pose &pose2, int synchronizor);
    // 处理从取图话题订阅到的image
    void image_callback(const sensor_msgs::msg::Image::SharedPtr debug_image);

    // void callback_Imgpose(const interfaces::msg::Imgpose::SharedPtr imgpose_in);

    void readpack_callback(const interfaces::msg::Readpack::SharedPtr readpack);

    // void callback_clocknow(const interfaces::msg::Clocknow::SharedPtr clock_now_temp);

    void callback_autoaim(interfaces::msg::Armor::SharedPtr armors_in);

    void callback_tovision(interfaces::msg::Tovision::SharedPtr tovision_in);

    void convert(interfaces::msg::Armor::SharedPtr armors_in);

    void armorFunc(interfaces::msg::Armor::SharedPtr armors_in);

    // void setSyncPredData(interfaces::msg::Sendpack &send_pack,int64_t clock_now);

    // void setTargetSolvePredData(interfaces::msg::Sendpack &send_pack, int64_t clock_now);

    void setArmorPredictdata(SendPack &send_pack, int64_t clock_now);

    void setDynamicHitOutpostNew(SendPack &send_pack, int64_t clock_now);

    void setFixHitOutpostNew(SendPack &send_pack, int64_t clock_now);

    void dynamic_follow_hit_outpost(void);

    void fix_hit_outpost(void);

    // void outpost_f_func(std::shared_ptr<interfaces::srv::Armor::Response> ROI);
    void timer_callback();
};

extern std::shared_ptr<AutoAim> autoaim_node;
extern cv::Mat testimg;
extern int USE_COMM;
extern bool DEBUG;
extern geometry_msgs::msg::PoseStamped pose_msg;
